#include<stdio.h>      /*标准输入输出定义*/
#include<stdlib.h>     /*标准函数库定义*/
#include<unistd.h>     /*Unix 标准函数定义*/
#include<termios.h>    /*PPSIX 终端控制定义*/
#include<string.h>
#include"com.h"
static int tank_com = 0;
void turn_left(int vel)
{
	char cmd[128] = {0};
	snprintf(cmd, 128, "!M %d %d\r", -vel, vel);
	write(tank_com, cmd , strlen(cmd));
}

void turn_right(int vel)
{
	char cmd[128] = {0};
	snprintf(cmd, 128, "!M %d %d\r", vel, -vel);
	write(tank_com, cmd , strlen(cmd));
}

void forward(int vel)
{
	char cmd[128] = {0};
	snprintf(cmd, 128, "!M %d %d\r", vel, vel);
	write(tank_com, cmd , strlen(cmd));
}

void backward(int vel)
{
	char cmd[128] = {0};

	snprintf(cmd, 128, "!M %d %d\r", -vel, -vel);
	write(tank_com, cmd , strlen(cmd));
}


void set_speed(int r_vel, int l_vel)
{
	l_vel = l_vel > 1000 ? 1000 : l_vel;
	r_vel = r_vel > 1000 ? 1000 : r_vel;

	char cmd[128] = {0};

	snprintf(cmd, 128, "!M %d %d\r", l_vel, r_vel);
	write(tank_com, cmd , strlen(cmd));
}

//x
//^
//|
//|
//y<----
//x 正方向运动
void linear_speed(int vel)
{
	if(vel > 1000)
	{
		vel = 1000;
	}

	if(vel < -1000)
	{
		vel = -1000;
	}

	char cmd[128] = {0};
	snprintf(cmd, 128, "!M %d %d\r", vel, vel);
	write(tank_com, cmd , strlen(cmd));
}
// theta 正方向,右手
void rotation_speed(int vel)
{
	if(vel > 1000)
	{
		vel = 1000;
	}

	if(vel < -1000)
	{
		vel = -1000;
	}

	char cmd[128] = {0};
	snprintf(cmd, 128, "!M %d %d\r", -vel, vel);
	write(tank_com, cmd , strlen(cmd));
}


void base_init(const char* port_name)
{
	int fd;                            //文件描述符
	int err;                           //返回调用函数的状态

	char send_buf[20] = "tiger john";
	fd = uart_open(port_name); //打开串口，返回文件描述符
	tank_com = fd;
	err = uart_set(fd, 115200, 0, 8, 1, 'N');
}
